import rclpy
from rclpy.node import Node
from hippo_control_msgs.msg import ActuatorSetpoint
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
import time
import random

class ActuatorCommandNode(Node):
    def __init__(self):
        super().__init__('actuator_command_node')
        
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE
        )
        
        self.thrust_sub = self.create_subscription(
            ActuatorSetpoint,
            '/bluerov00/thrust_setpoint_raw',
            self.thrust_callback,
            qos_profile
        )

        self.torque_sub = self.create_subscription(
            ActuatorSetpoint,
            '/bluerov00/torque_setpoint_raw',
            self.torque_callback,
            qos_profile
        )

        self.thrust_pub = self.create_publisher(
            ActuatorSetpoint,
            '/bluerov00/thrust_setpoint',
            qos_profile
        )

        self.torque_pub = self.create_publisher(
            ActuatorSetpoint,
            '/bluerov00/torque_setpoint',
            qos_profile
        )
        
        # 使用字典存储多个消息及其接收时间
        self.thrust_messages = {}  # {id: (msg, receive_time)}
        self.torque_messages = {}
        self.thrust_counter = 0
        self.torque_counter = 0
        
        self.timer = self.create_timer(0.1, self.timer_callback)
        
        self.get_logger().info("ActuatorCommandNode initialized with proper delay logic")

    def thrust_callback(self, msg):
        # 为每个消息分配唯一ID和时间戳
        msg_id = self.thrust_counter
        self.thrust_messages[msg_id] = (msg, time.time())
        self.thrust_counter += 1
        self.get_logger().info(f'Received thrust setpoint {msg_id}: x={msg.x}, y={msg.y}, z={msg.z}')

    def torque_callback(self, msg):
        msg_id = self.torque_counter
        self.torque_messages[msg_id] = (msg, time.time())
        self.torque_counter += 1
        self.get_logger().info(f'Received torque setpoint {msg_id}: x={msg.x}, y={msg.y}, z={msg.z}')

    def timer_callback(self):
        current_time = time.time()
        
        # 处理推力消息
        to_remove_thrust = []
        for msg_id, (msg, receive_time) in self.thrust_messages.items():
            if current_time - receive_time >= random.uniform(0.02, 0.04):  # 2秒延时
                self.thrust_pub.publish(msg)
                self.get_logger().info(f'PUBLISHED delayed thrust setpoint {msg_id}!')
                to_remove_thrust.append(msg_id)
        
        # 移除已处理的消息
        for msg_id in to_remove_thrust:
            del self.thrust_messages[msg_id]
        
        # 处理转矩消息
        to_remove_torque = []
        for msg_id, (msg, receive_time) in self.torque_messages.items():
            if current_time - receive_time >= random.uniform(0.02, 0.04):  # 2秒延时
                self.torque_pub.publish(msg)
                self.get_logger().info(f'PUBLISHED delayed torque setpoint {msg_id}!')
                to_remove_torque.append(msg_id)
        
        # 移除已处理的消息
        for msg_id in to_remove_torque:
            del self.torque_messages[msg_id]


def main(args=None):
    rclpy.init(args=args)
    actuator_command_node = ActuatorCommandNode()
    try:
        rclpy.spin(actuator_command_node)
    except KeyboardInterrupt:
        pass
    finally:
        actuator_command_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()